#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.hpp>
#include <turtlesim/msg/pose.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
/*
1，启动turtlesim_node
2，
*/
class TFDynamicBroadcastCppNode : public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_broadcast;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub;

public:
    TFDynamicBroadcastCppNode() : Node("TF_dynamic_broadcast_cpp_node")
    {
        RCLCPP_INFO(this->get_logger(), "TFDynamicBroadcastCppNode创建");
        dynamic_broadcast = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        pose_sub = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose", 10,
            std::bind(&TFDynamicBroadcastCppNode::do_pose, this, std::placeholders::_1));
    }

private:
    void do_pose(const turtlesim::msg::Pose &pose)
    {
        geometry_msgs::msg::TransformStamped ts;
        ts.header.stamp = this->now();
        ts.header.frame_id = "world";
        ts.child_frame_id = "turtle1";
        //
        ts.transform.translation.x = pose.x;
        ts.transform.translation.y = pose.y;
        ts.transform.translation.z = 0.0;
        //
        tf2::Quaternion qtn;
        qtn.setRPY(0.0, 0.0, pose.theta); // pose 接口 theta 就是yaw
        ts.transform.rotation.x = qtn.getX();
        ts.transform.rotation.y = qtn.y();
        ts.transform.rotation.z = qtn.getZ();
        ts.transform.rotation.w = qtn.getW();
        //
        dynamic_broadcast->sendTransform(ts);
    }
};
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TFDynamicBroadcastCppNode>());
    rclcpp::shutdown();
    return 0;
}